#include "rclcpp/rclcpp.hpp"
#include "node_constants.h"
#include "map_server_2d.h"
#include "publish_occupancy_grid_map.h"

class OccupancyGridPublisher : public rclcpp::Node {
public:
  OccupancyGridPublisher()
    : Node("publish_occupancy_grid_map") {
    
    // todo: use shared memery parameters
    this->declare_parameter<double>("resolution", 0.05);
    this->declare_parameter<bool>("include_frozen_submaps", true);
    this->declare_parameter<bool>("include_unfrozen_submaps", true);
    this->declare_parameter<int>("map_save_time_interval", 60);

    // 获取参数值
    double resolution = this->get_parameter("resolution").as_double();
    bool include_frozen = this->get_parameter("include_frozen_submaps").as_bool();
    bool include_unfrozen = this->get_parameter("include_unfrozen_submaps").as_bool();
    int save_interval = this->get_parameter("map_save_time_interval").as_int();

    // 参数检查
    if (!include_frozen && !include_unfrozen) {
      RCLCPP_FATAL(this->get_logger(), 
          "Ignoring both frozen and unfrozen submaps makes no sense.");
      throw std::runtime_error("Invalid parameter combination");
    }

    // 初始化节点
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(100),
      [this, resolution, save_interval]() {
        node_ = std::make_shared<slam_ros::PublishOccupancyGridMap>(
            this->shared_from_this(), resolution, save_interval);
        timer_->cancel();  
      });
  }

private:
  std::shared_ptr<slam_ros::PublishOccupancyGridMap> node_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<OccupancyGridPublisher>();
  
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  executor.spin();  
  
  rclcpp::shutdown();
  return 0;
}